#include <chrono>
#include <thread>

#include "action_interface/action/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using namespace std::literals::chrono_literals;

class RobotControl01 : public rclcpp::Node {
 public:
  using MoveRobot = action_interface::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;
  RobotControl01(std::string name, const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions())
      : Node(name, node_options) {
    RCLCPP_INFO(this->get_logger(), "Node: %s...", name.c_str());
    action_client_ = rclcpp_action::create_client<MoveRobot>(this, "move_robot");
    execute();
  }

  void execute() { std::thread{std::bind(&RobotControl01::send_goal, this)}.detach(); }

  void send_goal() {
    using namespace std::placeholders;
    if (!action_client_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }
    // 1,建立发送的消息对象
    auto goal_msg = MoveRobot::Goal();
    goal_msg.distance = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");
    // 2,建立发送目标选项对象
    auto send_goal_options = rclcpp_action::Client<MoveRobot>::SendGoalOptions();
    // 3,设置回调函数
    send_goal_options.goal_response_callback =
        std::bind(&RobotControl01::_goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&RobotControl01::_feedback_callback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&RobotControl01::_result_callback, this, _1);
    // 4,发送目标
    action_client_->async_send_goal(goal_msg, send_goal_options);
    RCLCPP_INFO(this->get_logger(), "Sent goal and exiting");
  }

 private:
  void _goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_INFO(this->get_logger(), "Goal was rejected by server...");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal was accepted by server,wating for result...");
    }
    RCLCPP_INFO(this->get_logger(), "response thread: %d", std::this_thread::get_id());
  }
  void _feedback_callback(GoalHandleMoveRobot::SharedPtr,
                          const std::shared_ptr<const MoveRobot::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback current pose is %f.", feedback->pose);
    RCLCPP_INFO(this->get_logger(), "Feedback thread: %d", std::this_thread::get_id());
  }
  void _result_callback(const GoalHandleMoveRobot::WrappedResult &result) {
    RCLCPP_INFO(this->get_logger(), "result thread: %d", std::this_thread::get_id());
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED: {
        RCLCPP_INFO(this->get_logger(), "Goal solve success.");
        return;
      }
      case rclcpp_action::ResultCode::ABORTED: {
        RCLCPP_INFO(this->get_logger(), "Goal was aborted.");
        return;
      }
      case rclcpp_action::ResultCode::CANCELED: {
        RCLCPP_INFO(this->get_logger(), "Goal was cancel.");
        return;
      }
      default: {
        RCLCPP_INFO(this->get_logger(), "Unknown result code.");
        return;
      }

        RCLCPP_INFO(this->get_logger(), "Result recievered pose is %f", result.result->pose);
    }
  }

 private:
  rclcpp_action::Client<MoveRobot>::SharedPtr action_client_{};
  rclcpp::TimerBase::SharedPtr timer_{};
};

int main(int argc, char **argv) {
  RCLCPP_INFO(rclcpp::get_logger("main"), "main thread: %d", std::this_thread::get_id());
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotControl01>("robot_control_01");
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}